/*
 * MecanumDriveSubsystem.cpp
 *
 *  Created on: Sep 24, 2011
 *      Author: David
 */

// Include files.
#include <cmath>
#include "MecanumDriveSubsystem.hpp"
#include "FpkException.hpp"
#include "FpkAssert.hpp"

// Use namespaces.
using namespace boost;

// Define static variables.
Gyro &MecanumDriveSubsystem::s_nullGyro(*((Gyro*)(NULL)));

MecanumDriveSubsystem::CartesianValue::CartesianValue() :
	m_x(0),
	m_y(0),
	m_rotation(0)
{
}

MecanumDriveSubsystem::CartesianValue::CartesianValue(double x, double y, double rotation) :
	m_x(x),
	m_y(y),
	m_rotation(rotation)
{
}

MecanumDriveSubsystem::DriveDistance::DriveDistance()
{
	m_driveMode = DriveModeNone;
	m_x = 0;
	m_y = 0;
	m_rotation = 0;
	m_threshold = 0;
}

MecanumDriveSubsystem::DriveDistance::DriveDistance
(
	DriveMode driveMode,
	double x,
	double y,
	double rotation,
	double threshold
)
{
	m_driveMode = driveMode;
	m_x = x;
	m_y = y;
	m_rotation = rotation;
	m_threshold = threshold;
}

double MecanumDriveSubsystem::DriveDistance::GetWheelDistance(const MecanumDriveSubsystem& drive, MotorPosition position)
{
	/* The following code in the block after
	 * the next block is derived from WPILib, which 
	 * is licenced under the licence in the file 
	 * "Licence-WPILib.txt" inside the project directory.
	 */
	
	if (m_driveMode == DriveModeNone)
		return 0;
	else if (m_driveMode == DriveModeCartesian)
	{
		switch (position)
		{
			double invertation;
			
			case MotorPositionLeftFront:
				invertation = GetMotorInvertationMultiplier(drive.m_motorInvertationLeftFront);
				return (m_x + m_y + m_rotation) * invertation;
				break;
			case MotorPositionLeftBack:
				invertation = GetMotorInvertationMultiplier(drive.m_motorInvertationLeftBack);
				return (-m_x + m_y + m_rotation) * invertation;
				break;
			case MotorPositionRightFront:
				invertation = GetMotorInvertationMultiplier(drive.m_motorInvertationRightFront);
				return (-m_x + m_y - m_rotation) * invertation;
				break;
			case MotorPositionRightBack:
				invertation = GetMotorInvertationMultiplier(drive.m_motorInvertationRightBack);
				return (m_x + m_y - m_rotation) * invertation;
				break;
			default:
				throw FpkException("Invalid Motor Position");
		}
	}
	else if (m_driveMode == DriveModePolar)
	{
		// TODO: Support Polar.
		
		throw FpkException("Not Implemented");
	}
	else
		throw FpkException("Invalid Drive Mode");
}

MecanumDriveSubsystem::ScaleModifier::ScaleModifier(double scale) :
	m_scale(scale)
{
}

void MecanumDriveSubsystem::ScaleModifier::Modify
(
	MecanumDriveSubsystem::CartesianValue &value
)
{
	value.m_x *= m_scale;
	value.m_y *= m_scale;
	value.m_rotation *= m_scale;
}

MecanumDriveSubsystem::DeadZoneModifier::DeadZoneModifier(double minimumAbsValue) :
	m_minimumAbsValue(minimumAbsValue)
{
}

void MecanumDriveSubsystem::DeadZoneModifier::Modify
(
	MecanumDriveSubsystem::CartesianValue &value
)
{
	//printf("In: X:%f, Y:%f, Z%f", value.m_x, value.m_y, value.m_rotation);
	// X Deadzone
	if (fabs(value.m_x) < m_minimumAbsValue)
		value.m_x = 0;
	else if(value.m_x >= m_minimumAbsValue)
		value.m_x = (1/(1 - m_minimumAbsValue)) * (value.m_x - m_minimumAbsValue);
	else
		value.m_x = (1/(1 - m_minimumAbsValue)) * (value.m_x + m_minimumAbsValue);
	
	// Y Deadzone
	if (fabs(value.m_y) < m_minimumAbsValue)
		value.m_y = 0;
	else if(value.m_y >= m_minimumAbsValue)
		value.m_y = (1/(1 - m_minimumAbsValue)) * (value.m_y - m_minimumAbsValue); 
	else
		value.m_y = (1/(1 - m_minimumAbsValue)) * (value.m_y + m_minimumAbsValue);
	
	// Rotation Deadzone
	if (fabs(value.m_rotation) < m_minimumAbsValue)
		value.m_rotation = 0;
	else if(value.m_rotation >= m_minimumAbsValue)
		value.m_rotation = (1/(1 - m_minimumAbsValue)) * (value.m_rotation - m_minimumAbsValue);
	else
		value.m_rotation = (1/(1 - m_minimumAbsValue)) * (value.m_rotation + m_minimumAbsValue);

	//printf(" Out: X:%f, Y:%f, Z%f\n", value.m_x, value.m_y, value.m_rotation);
}

MecanumDriveSubsystem::DriveStartState::DriveStartState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation
) :
	m_drive(drive),
	m_distance(mode, x, y, rotation, drive.GetDistanceThreshold())
{
}

MecanumDriveSubsystem::DriveStartState::DriveStartState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation,
	double threshold
) :
	m_drive(drive),
	m_distance(mode, x, y, rotation, threshold)
{
}

void MecanumDriveSubsystem::DriveStartState::InitializeImpl()
{
	switch (m_distance.m_driveMode)
	{
		case DriveModeCartesian:
			SetAsyncResult
			(
				m_drive.DriveDistanceByCartesian
				(
					m_distance.m_x,
					m_distance.m_y,
					m_distance.m_rotation
				)
			);
			break;
		case DriveModePolar:
			SetAsyncResult
			(
				m_drive.DriveDistanceByPolar
				(
					m_distance.m_x,
					m_distance.m_y,
					m_distance.m_rotation
				)
			);
			break;
		default:
			// TODO: Stop copying and pasting exception text.
			throw FpkException("Invalid Drive Mode");
			break;
	}
}

MecanumDriveSubsystem::DriveState::DriveState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation
) :
	AsyncResultEffectState<bool>(),
	m_stateStart(drive, mode, x, y, rotation)
{
	SetStartState(m_stateStart);
	AddResultDoneCodeMapping(true, StateDoneCode::GetSuccessCode());
}

MecanumDriveSubsystem::DriveState::DriveState
(
	MecanumDriveSubsystem &drive, 
	DriveMode mode, 
	double x, 
	double y, 
	double rotation,
	double threshold
) :
	AsyncResultEffectState<bool>(),
	m_stateStart(drive, mode, x, y, rotation, threshold)
{
	SetStartState(m_stateStart);
	AddResultDoneCodeMapping(true, StateDoneCode::GetSuccessCode());
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	GenericHID &leftJoystick, 
	GenericHID &rightJoystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = (leftJoystick.GetX() + rightJoystick.GetX()) / 2;
	value.m_y = -((leftJoystick.GetY() + rightJoystick.GetY()) / 2);
	value.m_rotation = (rightJoystick.GetY() - leftJoystick.GetY()) / 2;
	
	//printf("x input:%f, y input:%f, rotation:%f \n", value.m_x, value.m_y, value.m_rotation);
	//printf("Drive Mode: Two Joysticks, Tank Passive\n");
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::TankDriveWithActiveMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	if (leftJoystick.GetTrigger() && !rightJoystick.GetTrigger())
	{
		value.m_x = leftJoystick.GetX();
		value.m_y = -leftJoystick.GetY();
		value.m_rotation = 0;
	}
	else if (!leftJoystick.GetTrigger() && rightJoystick.GetTrigger())
	{
		value.m_x = rightJoystick.GetX();
		value.m_y = -rightJoystick.GetY();
		value.m_rotation = 0;
	}
	else if (leftJoystick.GetTrigger() && rightJoystick.GetTrigger())
	{
		value.m_x = 0;
		value.m_y = -((leftJoystick.GetY() + rightJoystick.GetY()) / 2);
		value.m_rotation = (rightJoystick.GetY() - leftJoystick.GetY()) / 2;
	}
	else
	{
		value.m_x = 0;
		value.m_y = 0;
		value.m_rotation = 0;
	}
	
	//printf("Drive Mode: Two Joysticks, Tank Passive \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = leftJoystick.GetX();
	value.m_y = -rightJoystick.GetY();
	value.m_rotation = rightJoystick.GetX();
	
	//printf("Drive Mode: Two Joysticks, Arcade Normal \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	GenericHID &leftJoystick,
	GenericHID &rightJoystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = rightJoystick.GetX();
	value.m_y = -rightJoystick.GetY();
	value.m_rotation = leftJoystick.GetX();
	
	//printf("Drive Mode: Two Joysticks, Arcade Modern \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;

	value.m_x = joystick.GetAxisValue(XboxJoystick::kLeftX);
	value.m_y =  - joystick.GetAxisValue(XboxJoystick::kLeftY);
	value.m_rotation = joystick.GetAxisValue(XboxJoystick::kRightX);
	
	//printf("Drive Mode: Xbox, Arcade Modern \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = joystick.GetAxisValue(XboxJoystick::kLeftX);
	value.m_y = - joystick.GetAxisValue(XboxJoystick::kRightY);
	value.m_rotation = joystick.GetAxisValue(XboxJoystick::kRightX);
	
	//printf("Drive Mode: Xbox, Arcade Normal \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	XboxJoystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = (joystick.GetAxisValue(XboxJoystick::kRightX) + joystick.GetAxisValue(XboxJoystick::kLeftX)) / 2;
	value.m_y = -(joystick.GetAxisValue(XboxJoystick::kRightY) + joystick.GetAxisValue(XboxJoystick::kLeftY)) / 2;
	value.m_rotation = (joystick.GetAxisValue(XboxJoystick::kRightY) - joystick.GetAxisValue(XboxJoystick::kLeftY)) / 2;
	
	//printf("Drive Mode: Xbox, Tank Passive \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	value.m_x = (joystick.GetAxisValue(Ps3Joystick::kRightX) + joystick.GetAxisValue(Ps3Joystick::kLeftX)) / 2;
	value.m_y = -(joystick.GetAxisValue(Ps3Joystick::kRightY) + joystick.GetAxisValue(Ps3Joystick::kLeftY)) / 2;
	value.m_rotation = (joystick.GetAxisValue(Ps3Joystick::kRightY) - joystick.GetAxisValue(Ps3Joystick::kLeftY)) / 2;
	
	//printf("Drive Mode: PS3, Tank Passive \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = joystick.GetAxisValue(Ps3Joystick::kLeftX);
	value.m_y =  - joystick.GetAxisValue(Ps3Joystick::kLeftY);
	value.m_rotation = joystick.GetAxisValue(Ps3Joystick::kRightX);
	
	//printf("Drive Mode: PS3, Arcade Modern \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = joystick.GetAxisValue(Ps3Joystick::kLeftX);
	value.m_y = joystick.GetAxisValue(Ps3Joystick::kRightY);
	value.m_rotation = joystick.GetAxisValue(Ps3Joystick::kRightX);
	
	//printf("Drive Mode: PS3, Arcade Normal \n");
	
	return value;
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::TiltDriveStyleWithMecanumValueFetcher
(
	Ps3Joystick &joystick
)
{
	MecanumDriveSubsystem::CartesianValue value;
	
	value.m_x = -joystick.GetAxisValue(Ps3Joystick::kLeftRightTilt);
	value.m_y = -joystick.GetAxisValue(Ps3Joystick::kFrontBackTilt);
	value.m_rotation = joystick.GetAxisValue(Ps3Joystick::kRightX);
	
	//printf("%f, %f, %f \n", value.m_x, value.m_y, value.m_rotation);
	//printf("Drive Mode: PS3, Tilt Style \n");
	
	return value;
}

void MecanumDriveSubsystem::DriveByCartesian
(
	MecanumDriveSubsystem &drive, 
	CartesianValue &value
)
{
	drive.DriveByCartesian(value.m_x, value.m_y, value.m_rotation);
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	UINT32 leftFrontMotorChannel, 
	UINT32 leftBackMotorChannel, 
	UINT32 rightFrontMotorChannel, 
	UINT32 rightBackMotorChannel
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotorChannel,
		leftBackMotorChannel,
		rightFrontMotorChannel,
		rightBackMotorChannel
	)
	, m_gyro(s_nullGyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(s_nullGyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar *leftFrontMotor, 
	CANJaguar *leftBackMotor,
	CANJaguar *rightFrontMotor, 
	CANJaguar *rightBackMotor,
	Gyro *gyro
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(*gyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(s_nullGyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

MecanumDriveSubsystem::MecanumDriveSubsystem
(
	CANJaguar &leftFrontMotor, 
	CANJaguar &leftBackMotor,
	CANJaguar &rightFrontMotor, 
	CANJaguar &rightBackMotor,
	Gyro &gyro
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(gyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController *leftFrontMotor, 
	SpeedController *leftBackMotor,
	SpeedController *rightFrontMotor, 
	SpeedController *rightBackMotor
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(s_nullGyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Constructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::MecanumDriveSubsystem
(
	SpeedController &leftFrontMotor, 
	SpeedController &leftBackMotor,
	SpeedController &rightFrontMotor, 
	SpeedController &rightBackMotor
) :
	FourMotorTankDriveSubsystem
	(
		leftFrontMotor,
		leftBackMotor,
		rightFrontMotor,
		rightBackMotor
	)
	, m_gyro(s_nullGyro)
	, m_driveDistance()
	, m_driveDistanceAsyncResult()
{
	Startup();
}

/**
 *  Destructor for MecanumDriveSubsystem.
 */
MecanumDriveSubsystem::~MecanumDriveSubsystem()
{
	delete m_safetyHelper;
}

void MecanumDriveSubsystem::Stop()
{
	// Call base implimentation.
	FourMotorTankDriveSubsystem::Stop();
}

/**
 *  Drive the specified amount on a cartesian plane.
 *  
 *  Testing shows this function works.
 *  
 *  @param x The amount to drive on the x-axis.
 *  @prarm y The amount to drive on the y-axis.
 *  @prarm rotation The amount to rotate.
 */
void MecanumDriveSubsystem::DriveByCartesian(double x, double y, double rotation)
{
	if (m_driveDistanceAsyncResult.get() != NULL)
	{
		CancelCurrentDriveDistance();
		
		SetControlMode(ControlModeSpeed);
	}
	
	// Apply reversal if needed.
	if (m_reversed)
	{
		x *= -1;
		y *= -1;
	}
	
	rotation = CompensateWithGyro(CartesianValue(x, y, rotation)).m_rotation;
	
	// Use "-y" to negate RobotDrive's negation
	SmartDashboard::Log(-y, "DriveY");
	SmartDashboard::Log(x, "DriveX");
	SmartDashboard::Log(rotation, "DriveRotation");
	m_drive.MecanumDrive_Cartesian(x, -y, rotation);
	m_safetyHelper->Feed();
	m_driveUpdated = true;
	//printf("actual motor speed : %f ", m_cjLeftBack.Get());
	//printf("requested motor speed : %f \n", (-x + y + rotation) * GetMotorInvertationMultiplier(m_motorInvertationRightBack) * m_maxOutput);
}

/**
 *  Drive the specified amount using polar coordinates.
 *  
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to drive.
 *  @prarm direction The direction to drive.
 *  @prarm rotation The amount to rotate.
 */
void MecanumDriveSubsystem::DriveByPolar(double magnitude, double direction, double rotation)
{
	if(m_driveDistanceAsyncResult.get() != NULL)
	{
		CancelCurrentDriveDistance();
		// TODO: Support other modes.
		SetControlMode(ControlModeSpeed);
	}
	
	// TODO: Support reversals for DriveByPolar.
	// TODO: Use gyro to compensate.
	
	m_drive.MecanumDrive_Polar(magnitude, direction, rotation);
	m_safetyHelper->Feed();
	m_driveUpdated = true;
}

/**
 *  Drive a distance specified by values in cartesian form.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 * 
 *  This function doesn't seem to work.
 *  
 *  @param x Distance to move on the X axis reletive to the robot.
 *  @param y Distance to move on the Y axis reletive to the robot.
 *  @param rotation Amount to rotate.
 *  
 *  @returns A shared pointer to an ayncronous result of type bool.
 */
shared_ptr<AsyncResult<bool> > MecanumDriveSubsystem::DriveDistanceByCartesian(double x, double y, double rotation)
{
	return DriveDistanceByCartesian(x, y, rotation, GetDistanceThreshold());
}

shared_ptr<AsyncResult<bool> > MecanumDriveSubsystem::DriveDistanceByCartesian(double x, double y, double rotation, double threshold)
{
	if (m_usesCanJaguars)
	{
		CancelCurrentDriveDistance();
		SetControlMode(ControlModeDistance);
		
		m_driveDistance.m_driveMode = DriveModeCartesian;
		m_driveDistance.m_x = x;
		m_driveDistance.m_y = y;
		m_driveDistance.m_rotation = rotation;
		m_driveDistance.m_threshold = threshold;
		
		DriveDistanceByCartesianInternal();
		
		m_driveDistanceAsyncResult = shared_ptr<DriveDistanceAsyncResult>(new DriveDistanceAsyncResult(m_driveDistance));
		return m_driveDistanceAsyncResult;
	}
	else
	{
		throw FpkException("Doesn't use CANJaguars!");
	}
}

/**
 *  Drive a distance specified by values in polar form.
 *  
 *  Currently only works if CANJaguars are used.
 *  If CANJaguars aren't being used, an exception will be thrown.
 * 
 *  This function is untested.
 *  
 *  @param magnitude The magnitude to move.
 *  @param direction The direction to move.
 *  @param rotation The amount to rotate.
 *  
 *  @returns A shared pointer to an ayncronous result of type bool.
 */
shared_ptr<AsyncResult<bool> > MecanumDriveSubsystem::DriveDistanceByPolar(double magnitude, double direction, double rotation)
{
	// TODO: Support Polar.
	
	CancelCurrentDriveDistance();
	SetControlMode(ControlModeDistance);
	
	m_driveDistance.m_driveMode = DriveModePolar;
	m_driveDistance.m_x = magnitude;
	m_driveDistance.m_y = direction;
	m_driveDistance.m_rotation = rotation;
	
	m_drive.MecanumDrive_Polar(magnitude, direction, rotation);
	m_safetyHelper->Feed();
	
	m_driveDistanceAsyncResult = shared_ptr<DriveDistanceAsyncResult>(new DriveDistanceAsyncResult(m_driveDistance));
	return m_driveDistanceAsyncResult;
}

void MecanumDriveSubsystem::Periodic()
{
	FourMotorTankDriveSubsystem::Periodic();
	
	if (m_driveDistanceAsyncResult.get() != NULL)
	{
		bool finished = false;
		
		if (m_driveDistance.m_driveMode == DriveModeNone)
			finished = true;
		else
		{
			switch(m_driveDistance.m_driveMode)
			{
				case DriveModeCartesian:
					DriveDistanceByCartesianInternal();
					break;
				default:
					throw FpkException("Not implemented");
			}
			
			if (m_usesCanJaguars)
			{
				double wheelDistanceLeftFront = 
					m_driveDistance.GetWheelDistance(*this, MotorPositionLeftFront);
				double wheelDistanceLeftBack = 
					m_driveDistance.GetWheelDistance(*this, MotorPositionLeftBack);
				double wheelDistanceRightFront = 
					m_driveDistance.GetWheelDistance(*this, MotorPositionRightFront);
				double wheelDistanceRightBack = 
					m_driveDistance.GetWheelDistance(*this, MotorPositionRightBack);
				
				finished = true;
				
				double threshold = m_driveDistance.m_threshold;
				
				finished = finished && (fabs(m_cjLeftFront.GetPosition() - wheelDistanceLeftFront) <= threshold);
				printf("m_cjLeftFront.GetPosition() is %f\n", float(m_cjLeftFront.GetPosition()));
				printf("wheelDistanceLeftFront is %f\n", float(wheelDistanceLeftFront));
				printf("Difference: %f\n", float(fabs(m_cjLeftFront.GetPosition() - wheelDistanceLeftFront)));
				printf("Finished: %d\n", int(finished));
				finished = finished && (fabs(m_cjLeftBack.GetPosition() - wheelDistanceLeftBack) <= threshold);
				printf("Difference: %f\n", float(fabs(m_cjLeftBack.GetPosition() - wheelDistanceLeftBack)));
				printf("Finished: %d\n", int(finished));
				finished = finished && (fabs(m_cjRightFront.GetPosition() - wheelDistanceRightFront) <= threshold);
				printf("Difference: %f\n", float(fabs(m_cjRightFront.GetPosition() - wheelDistanceRightFront)));
				printf("Finished: %d\n", int(finished));
				finished = finished && (fabs(m_cjRightBack.GetPosition() - wheelDistanceRightBack) <= threshold);
				printf("Difference: %f\n", float(fabs(m_cjRightBack.GetPosition() - wheelDistanceRightBack)));
				printf("Finished: %d\n", int(finished));
			}
			else
			{
				throw FpkException("CAN Jaguars Not Used");
			}
		}
		
		if (finished)
		{
			printf("Drive: Distance Drive Finished!\n");
			
			m_driveDistanceAsyncResult->Finish(true);
			m_driveDistanceAsyncResult.reset();
			SetControlMode(ControlModeSpeed);
		}
	}
}

MecanumDriveSubsystem::CartesianValue MecanumDriveSubsystem::CompensateWithGyro(CartesianValue value)
{
	static const double c_coefficient = 1/20; 
	
	if (&m_gyro != NULL)
	{
		if (value.m_rotation == 0)
			m_gyroAnglePrevious = m_gyro.GetAngle();
		else
			value.m_rotation = (m_gyroAnglePrevious - m_gyro.GetAngle())*c_coefficient;
	}
	
	return value;
}

void MecanumDriveSubsystem::CancelCurrentDriveDistance()
{
	if (m_driveDistanceAsyncResult != NULL)
	{
		m_driveDistanceAsyncResult->Cancel();
		m_driveDistanceAsyncResult.reset();
	}
}

void MecanumDriveSubsystem::DriveDistanceByCartesianInternal()
{
	FpkAssert(m_driveDistance.m_driveMode == DriveModeCartesian);
	
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */

	UINT8 syncGroup = 0x80;
	
	double x = m_driveDistance.m_x;
	double y = m_driveDistance.m_y;
	double rotation = m_driveDistance.m_rotation;
	
	// TODO: Use gyro to compensate.
	
	double leftFront = (x + y + rotation) * GetMotorInvertationMultiplier(m_motorInvertationLeftFront);
	double rightFront = (-x + y - rotation) * GetMotorInvertationMultiplier(m_motorInvertationRightFront);
	double leftBack = (-x + y + rotation) * GetMotorInvertationMultiplier(m_motorInvertationLeftBack);
	double rightBack = (x + y - rotation) * GetMotorInvertationMultiplier(m_motorInvertationRightBack); 
	
	m_cjLeftFront.Set(leftFront, syncGroup);
	m_cjRightFront.Set(rightFront, syncGroup);
	m_cjLeftBack.Set(leftBack, syncGroup);
	m_cjRightBack.Set(rightBack, syncGroup);
	
	CANJaguar::UpdateSyncGroup(syncGroup);
	
	m_safetyHelper->Feed();
	m_driveUpdated = true;
	
	printf("REMOVE THIS AFTER!  Drive: %f %f\n", leftFront, rightFront);
	printf("                           %f %f\n", leftBack, rightBack);
	printf("                  Current: %f %f\n", m_cjLeftFront.GetPosition(), m_cjRightFront.GetPosition());
	printf("                           %f %f\n", m_cjLeftBack.GetPosition(), m_cjRightBack.GetPosition());
}

void MecanumDriveSubsystem::Startup()
{
	m_gyroAnglePrevious = 0;
}



MecanumDriveSubsystem::DriveDistanceAsyncResult::DriveDistanceAsyncResult(DriveDistance &driveDistance) :
	m_isFinished(false),
	m_driveDistance(driveDistance)
{
}

bool MecanumDriveSubsystem::DriveDistanceAsyncResult::GetIsFinished() const
{
	return m_isFinished;
}

void MecanumDriveSubsystem::DriveDistanceAsyncResult::Cancel()
{
	if (!m_isFinished)
	{
		m_driveDistance.m_driveMode = DriveModeNone;
		Finish(false);
	}
}

bool MecanumDriveSubsystem::DriveDistanceAsyncResult::GetResult() const
{
	if (m_isFinished)
		return m_result;
	else
	{
		throw FpkException("Getting result from unfinished async operation.");
	}
}

void MecanumDriveSubsystem::DriveDistanceAsyncResult::Finish(bool result)
{
	m_isFinished = true;
	m_result = result;
}
